/**
 * @file ThetaStarPlanner.cpp
 * @brief Theta*算法路径规划器实现 - 修复参数版本
 */

#include "planners/ThetaStarPlanner.h"
#include <queue>
#include <unordered_set>
#include <chrono>
#include <iostream>

namespace PathPlanning
{
ThetaStarPlanner::ThetaStarPlanner(std::shared_ptr<Map> map) : PlannerBase(map)
{
    config_ = std::make_shared<ThetaStarConfig>();
    std::cout << "ThetaStarPlanner initialized" << std::endl;
}

PlanningResult ThetaStarPlanner::plan(const Point& start, const Point& goal)
{
    std::cout << "ThetaStarPlanner::plan started" << std::endl;
    PlanningResult result;
    auto           startTime = std::chrono::high_resolution_clock::now();

    if (!validateStartGoal(start, goal))
    {
        result.message = "Invalid start or goal position";
        return result;
    }

    auto thetaStarConfig = std::static_pointer_cast<ThetaStarConfig>(config_);

    // 初始化数据结构
    std::priority_queue<std::shared_ptr<ThetaStarNode>, std::vector<std::shared_ptr<ThetaStarNode>>,
                        ThetaStarNodeCompare>
        openSet;

    std::unordered_map<GridCell, std::shared_ptr<ThetaStarNode>, GridCellHash> allNodes;
    std::unordered_set<GridCell, GridCellHash>                                 closedSet;

    // 创建起点节点
    GridCell startCell = map_->worldToGrid(start);
    auto     startNode = std::make_shared<ThetaStarNode>(startCell, start);
    startNode->gCost   = 0.0;
    startNode->fCost   = calculateHeuristic(start, goal) * thetaStarConfig->weight;

    openSet.push(startNode);
    allNodes[startCell] = startNode;

    GridCell goalCell  = map_->worldToGrid(goal);
    auto     goalNode  = std::make_shared<ThetaStarNode>(goalCell, goal);
    allNodes[goalCell] = goalNode;

    std::cout << "Start cell: (" << startCell.x << ", " << startCell.y << ")" << std::endl;
    std::cout << "Goal cell: (" << goalCell.x << ", " << goalCell.y << ")" << std::endl;
    std::cout << "Map bounds: 0-" << map_->getWidth() - 1 << " x 0-" << map_->getHeight() - 1 << std::endl;

    // 检查起点和终点是否为障碍物
    if (map_->isObstacle(startCell.x, startCell.y))
    {
        std::cout << "ERROR: Start cell is obstacle!" << std::endl;
        result.message = "Start position is in obstacle";
        return result;
    }
    if (map_->isObstacle(goalCell.x, goalCell.y))
    {
        std::cout << "ERROR: Goal cell is obstacle!" << std::endl;
        result.message = "Goal position is in obstacle";
        return result;
    }

    int       iterations    = 0;
    const int maxIterations = 100000;

    while (!openSet.empty() && iterations < maxIterations)
    {
        iterations++;

        auto currentNode = openSet.top();
        openSet.pop();

        GridCell currentCell = currentNode->gridCell;

        // 检查是否已经在关闭列表中
        if (closedSet.find(currentCell) != closedSet.end())
        {
            continue;
        }

        // 添加到关闭列表
        closedSet.insert(currentCell);
        result.exploredNodes.push_back(Pose(currentNode->position.x, currentNode->position.y, 0));

        std::cout << "Processing cell: (" << currentCell.x << ", " << currentCell.y
                  << "), gCost: " << currentNode->gCost << ", fCost: " << currentNode->fCost << std::endl;

        // 检查是否到达目标
        if (currentCell.x == goalCell.x && currentCell.y == goalCell.y)
        {
            std::cout << "Theta*: Goal reached at iteration " << iterations << std::endl;
            result.success = true;
            result.path    = reconstructPath(allNodes, currentNode);
            break;
        }

        // 可视化回调
        if (iterations % 100 == 0)
        {
            result.iterations = iterations;
            PlannerBase::callVisualizationCallback(result);
            std::cout << "Theta*: Iteration " << iterations << ", open set: " << openSet.size()
                      << ", closed set: " << closedSet.size() << std::endl;
        }

        // 遍历邻居节点
        auto neighbors = getNeighbors(currentCell, thetaStarConfig->eightConnected);
        std::cout << "Found " << neighbors.size() << " neighbors" << std::endl;

        for (const auto& neighborCell : neighbors)
        {
            // 检查邻居是否为障碍物
            if (map_->isObstacle(neighborCell.x, neighborCell.y))
            {
                continue;
            }

            // 检查邻居是否在关闭列表中
            if (closedSet.find(neighborCell) != closedSet.end())
            {
                continue;
            }

            Point neighborWorld = map_->gridToWorld(neighborCell);

            // 查找或创建邻居节点
            std::shared_ptr<ThetaStarNode> neighborNode;
            auto                           neighborIt = allNodes.find(neighborCell);
            if (neighborIt == allNodes.end())
            {
                neighborNode           = std::make_shared<ThetaStarNode>(neighborCell, neighborWorld);
                neighborNode->gCost    = std::numeric_limits<double>::max();
                allNodes[neighborCell] = neighborNode;
            }
            else
            {
                neighborNode = neighborIt->second;
            }

            // Theta*核心：更新顶点 - 使用正确的参数
            double oldGCost = neighborNode->gCost;
            updateVertex(currentNode, neighborNode, goalNode, openSet, allNodes);

            // 如果代价有改进，加入开放列表
            if (neighborNode->gCost < oldGCost)
            {
                neighborNode->fCost =
                    neighborNode->gCost + calculateHeuristic(neighborWorld, goal) * thetaStarConfig->weight;
                openSet.push(neighborNode);
                std::cout << "Updated neighbor (" << neighborCell.x << ", " << neighborCell.y
                          << "), gCost: " << neighborNode->gCost << std::endl;
            }
        }

        // 第一次迭代后检查开放列表状态
        if (iterations == 1)
        {
            std::cout << "After first iteration - Open set size: " << openSet.size()
                      << ", Closed set size: " << closedSet.size() << std::endl;
            if (openSet.empty())
            {
                std::cout << "WARNING: Open set is empty after first iteration!" << std::endl;
                std::cout << "Start neighbors count: " << neighbors.size() << std::endl;

                // 调试：检查起点周围的邻居状态
                for (const auto& neighbor : neighbors)
                {
                    bool isObstacle = map_->isObstacle(neighbor.x, neighbor.y);
                    std::cout << "Neighbor (" << neighbor.x << ", " << neighbor.y
                              << ") - Obstacle: " << (isObstacle ? "Yes" : "No") << std::endl;
                }
            }
        }

        double distanceToGoal = euclideanDistance(currentNode->position, goal);
        if (distanceToGoal < 1.0)
        { // 1米范围内都认为是目标
            std::cout << "Theta*: Goal reached at iteration " << iterations << " (distance: " << distanceToGoal << ")"
                      << std::endl;
            result.success = true;
            result.path    = reconstructPath(allNodes, currentNode);
            break;
        }

        // 或者使用网格坐标检查
        if (currentCell.x == goalCell.x && currentCell.y == goalCell.y)
        {
            std::cout << "Theta*: Goal reached at iteration " << iterations << std::endl;
            result.success = true;
            result.path    = reconstructPath(allNodes, currentNode);
            break;
        }
    }

    auto endTime        = std::chrono::high_resolution_clock::now();
    result.planningTime = std::chrono::duration<double>(endTime - startTime).count();
    result.iterations   = iterations;

    if (!result.success)
    {
        result.message = "Path not found within iteration limit";
        std::cout << "Theta* failed after " << iterations << " iterations" << std::endl;
        std::cout << "Final open set size: " << openSet.size() << std::endl;
        std::cout << "Final closed set size: " << closedSet.size() << std::endl;

        // 调试信息
        if (closedSet.size() == 1)
        {
            std::cout << "DEBUG: Only start node was expanded. Possible issues:" << std::endl;
            std::cout << "1. All neighbors are obstacles" << std::endl;
            std::cout << "2. Neighbor generation is broken" << std::endl;
            std::cout << "3. Cost calculation prevents expansion" << std::endl;
        }
    }
    else
    {
        // 计算路径长度
        for (size_t i = 1; i < result.path.size(); ++i)
        {
            result.pathLength += euclideanDistance(result.path[i - 1].toPoint(), result.path[i].toPoint());
        }
        result.message = "Path found successfully";
        std::cout << "Theta* found path with " << result.path.size() << " points" << std::endl;
    }

    return result;
}

double ThetaStarPlanner::calculateHeuristic(const Point& from, const Point& to)
{
    auto thetaStarConfig = std::static_pointer_cast<ThetaStarConfig>(config_);

    if (thetaStarConfig->heuristicType == "manhattan")
    {
        return manhattanDistance(from, to);
    }
    else
    {
        return euclideanDistance(from, to);
    }
}

bool ThetaStarPlanner::hasLineOfSight(const GridCell& a, const GridCell& b)
{
    // 简化版视线检查：只检查直线上的点
    int x0 = a.x, y0 = a.y;
    int x1 = b.x, y1 = b.y;

    int dx  = std::abs(x1 - x0);
    int dy  = std::abs(y1 - y0);
    int sx  = (x0 < x1) ? 1 : -1;
    int sy  = (y0 < y1) ? 1 : -1;
    int err = dx - dy;

    int x = x0, y = y0;

    int       steps    = 0;
    const int maxSteps = 1000; // 防止无限循环

    while (steps < maxSteps)
    {
        steps++;

        // 跳过起点和终点
        if (!(x == x0 && y == y0) && !(x == x1 && y == y1))
        {
            // 检查当前网格是否为障碍物
            if (map_->isObstacle(x, y))
            {
                return false;
            }
        }

        if (x == x1 && y == y1)
        {
            break;
        }

        int e2 = 2 * err;
        if (e2 > -dy)
        {
            err -= dy;
            x += sx;
        }
        if (e2 < dx)
        {
            err += dx;
            y += sy;
        }
    }

    return true;
}

void ThetaStarPlanner::updateVertex(
    const std::shared_ptr<ThetaStarNode>& current, const std::shared_ptr<ThetaStarNode>& neighbor,
    const std::shared_ptr<ThetaStarNode>&                                       goalNode,
    std::priority_queue<std::shared_ptr<ThetaStarNode>, std::vector<std::shared_ptr<ThetaStarNode>>,
                        ThetaStarNodeCompare>&                                  openSet,
    std::unordered_map<GridCell, std::shared_ptr<ThetaStarNode>, GridCellHash>& allNodes)
{
    auto thetaStarConfig = std::static_pointer_cast<ThetaStarConfig>(config_);

    // 方法1：通过当前节点到达邻居
    double costThroughCurrent = current->gCost + euclideanDistance(current->position, neighbor->position);

    // 方法2：Theta* - 通过祖父节点到达邻居（如果有视线）
    if (current->parent != nullptr)
    {
        auto parent = std::static_pointer_cast<ThetaStarNode>(current->parent);

        if (hasLineOfSight(parent->gridCell, neighbor->gridCell))
        {
            double costThroughParent = parent->gCost + euclideanDistance(parent->position, neighbor->position);

            if (costThroughParent < costThroughCurrent)
            {
                costThroughCurrent = costThroughParent;
                neighbor->parent   = parent; // 直接连接到祖父节点
            }
        }
    }

    // 如果找到更优路径，更新邻居节点
    if (costThroughCurrent < neighbor->gCost)
    {
        neighbor->gCost = costThroughCurrent;
        // 注意：parent 已经在上面设置了（如果是通过祖父节点）
        if (neighbor->parent == nullptr)
        {
            neighbor->parent = current; // 如果没有通过祖父节点，使用当前节点作为父节点
        }
    }
}

std::vector<Pose> ThetaStarPlanner::reconstructPath(
    const std::unordered_map<GridCell, std::shared_ptr<ThetaStarNode>, GridCellHash>& allNodes,
    const std::shared_ptr<ThetaStarNode>&                                             endNode)
{
    std::vector<Pose> path;
    auto              currentNode = endNode;

    while (currentNode != nullptr)
    {
        path.push_back(Pose(currentNode->position.x, currentNode->position.y, 0));
        if (currentNode->parent)
        {
            currentNode = std::static_pointer_cast<ThetaStarNode>(currentNode->parent);
        }
        else
        {
            break;
        }
    }

    std::reverse(path.begin(), path.end());

    std::cout << "Reconstructed path with " << path.size() << " points" << std::endl;

    // Theta*路径通常已经比较平滑，但可以进一步优化
    return smoothPath(path);
}

} // namespace PathPlanning